'''
Created on Dec 14, 2009

@author: grasping
'''
def getCylinder(heightInM,env):
    createCylinderXML(heightInM)
    cyl = env.ReadKinBodyXMLFile("/tmp/cylinder.xml")
    return cyl

def getAxisMarker(env):
    createAxisMarker()
    axes = env.ReadKinBodyXMLFile("/tmp/axis.xml")
    return axes

def createCylinderXML(heightInM):
    fp = file("/tmp/cylinder.xml","w")
    fp.write('<?xml version="1.0" encoding="utf-8"?>')
    fp.write("\n")
    fp.write('<KinBody name="Cylinder">')
    fp.write('<Translation>0.98 0 -0.5710</Translation>')
    fp.write('<Body name="Body1" type="Dynamic">')
    fp.write('<Geom type="cylinder">')
    fp.write('<DiffuseColor>0.8 0 0</DiffuseColor>')
    fp.write('<RotationAxis>1 0 0 90</RotationAxis>')
    fp.write('<Translation>0 0 0.052</Translation>')
    fp.write('<Radius>0.2</Radius>')
    fp.write('<Height>'+str(heightInM)+'</Height>')
    fp.write('</Geom>')
    fp.write('</Body>')
    fp.write('</KinBody>')
    fp.close()

def createAxisMarker():
    fp = file("/tmp/axis.xml",'w')
    fp.write('<?xml version="1.0" encoding="utf-8"?>')
    fp.write("\n")
    fp.write('<KinBody name="Axis">')
    fp.write('<Body name="body" type="dynamic">')
    fp.write('<Geom type="trimesh">')
    fp.write('<data>models/axes/cross.iv</data>')
    fp.write('<render>models/axes/cross.iv</render>')
    fp.write('</Geom>')
    fp.write('</Body>')
    fp.write('</KinBody>')
    fp.close()

def createContactMarker(colorval):
    fp = file("/tmp/contact.xml",'w')
    fp.write('<?xml version="1.0" encoding="utf-8"?>')
    fp.write("\n")
    fp.write('<KinBody name="Axis">')
    fp.write('<Body name="contact" type="dynamic">')
    fp.write('<Geom type="sphere">')
    fp.write('<DiffuseColor>'+colorval+'</DiffuseColor>')
    fp.write('<Radius>0.005</Radius>')
    fp.write('</Geom>')
    fp.write('</Body>')
    fp.write('</KinBody>')
    fp.close()

def getContactMarker(env, colorval):
    createContactMarker(colorval)
    cm = env.ReadKinBodyXML("/tmp/contact.xml")
    return cm

def get_xml_ball(point):
    """Given a 3d point, creates an XML string representing a ball in OpenRAVE.
    
    Keyword arguments:
    point -- a list-like object containing a 3d point, (x,y,z)
    
    Usage:
    env = orp.Environment()
    ball_str = get_xml_ball((0,0,0))
    ball = env.GetKinBodyXMLData(ball_str)
    """

    string = '<?xml version="1.0" encoding="utf-8"?>'
    string += '<KinBody name="TestChain">'
    string += '<Body name="Body1" type="dynamic">'
    string += '<Geom type="sphere">'
    string += '<Radius>0.003</Radius>'
    string += '<Translation>' + ' '.join(str(p) for p in point) + '</Translation>'
    string += '</Geom>'
    string += '</Body>'
    string += '</KinBody>'
    return string